/*
 *  Player Java Client 3 - SimulationInterface.java
 *  Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * $Id: SimulationInterface.java 125 2011-03-24 02:24:05Z corot $
 *
 */
package javaclient3;

import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;

import javaclient3.structures.PlayerMsgHdr;
import javaclient3.structures.PlayerPose2d;
import javaclient3.structures.PlayerPose3d;
import javaclient3.structures.simulation.PlayerSimulationData;
import javaclient3.structures.simulation.PlayerSimulationPose2dReq;
import javaclient3.structures.simulation.PlayerSimulationPose3dReq;
import javaclient3.xdr.OncRpcException;
import javaclient3.xdr.XdrBufferDecodingStream;
import javaclient3.xdr.XdrBufferEncodingStream;

/**
 * Player devices may either be real hardware or virtual devices generated by
 * a simulator such as Stage or Gazebo. This interface provides direct access
 * to a simulator.
 * <br><br>
 * This interface doesn't do much yet. It is in place to later support things
 * like pausing and restarting the simulation clock, saving and loading, etc.
 * It is documented because it is used by the stg_simulation driver; required
 * by all stageclient drivers (stg_*).
 * @author Radu Bogdan Rusu
 * @version
 * <ul>
 *      <li>v3.0 - Player 3.0 supported
 * </ul>
 */
public class SimulationInterface extends PlayerDevice {

    private static final boolean isDebugging = PlayerClient.isDebugging;
    // Logging support
    private Logger logger = Logger.getLogger(SimulationInterface.class.getName());
    private PlayerSimulationData data;
    private boolean readyData = false;
    private PlayerSimulationPose2dReq psp2dreq;
    private PlayerSimulationPose3dReq psp3dreq;
    private boolean readyPsp2dreq = false;
    private boolean readyPsp3dreq = false;

    /**
     * Constructor for SimulationInterface.
     * @param pc a reference to the PlayerClient object
     */
    public SimulationInterface(PlayerClient pc) {
        super(pc);
    }

    /**
     * Read the simulation data
     */
    public synchronized void readData(PlayerMsgHdr header) {
        try {
            this.timestamp = header.getTimestamp();

            data = new PlayerSimulationData();
            // Buffer for reading data
            byte[] buffer = new byte[4];
            is.readFully(buffer);

            // Begin decoding the XDR buffer
            XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
            xdr.beginDecoding();
            data.setData(xdr.xdrDecodeByte());
            xdr.endDecoding();
            xdr.close();

            readyData = true;
        } catch (IOException e) {
            throw new PlayerException("[Simulation] : Error reading payload: "
                    + e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException("[Simulation] : Error while XDR-decoding payload: "
                    + e.toString(), e);
        }
    }

    /**
     * Get the 2D pose of a named simulation object.
     * @return an object of type PlayerSimulationPose2dReq containing the required pose data
     */
    public PlayerSimulationPose2dReq getSimulationPose2D() {
        return psp2dreq;
    }

    /**
     * Get the 3D pose of a named simulation object.
     * @return an object of type PlayerSimulationPose3dReq containing the required pose data
     */
    public PlayerSimulationPose3dReq getSimulationPose3D() {
        return psp3dreq;
    }

    /**
     * Check if 2D pose data is available.
     * @return true if ready, false if not ready
     */
    public boolean isPose2DReady() {
        if (readyPsp2dreq) {
            readyPsp2dreq = false;
            return true;
        }
        return false;
    }

    /**
     * Check if 3D pose data is available.
     * @return true if ready, false if not ready
     */
    public boolean isPose3DReady() {
        if (readyPsp3dreq) {
            readyPsp3dreq = false;
            return true;
        }
        return false;
    }

    /**
     * Get the data.
     * @return an object of type PlayerSimulationData containing the required data
     */
    public PlayerSimulationData getData() {
        return this.data;
    }

    /**
     * Check if data is available.
     * @return true if ready, false if not ready
     */
    public boolean isDataReady() {
        if (readyData) {
            readyData = false;
            return true;
        }
        return false;
    }

    /**
     * Placeholder for command.
     * <br><br>
     * See the player_simulation_cmd structure from player.h
     * @param cmd a single byte of as-yet-unspecified command.
     */
    public void setPosition(byte cmd) {
        try {
            sendHeader(PLAYER_MSGTYPE_CMD, 0, 4);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream(4);
            xdr.beginEncoding(null, 0);
            xdr.xdrEncodeByte((byte) cmd);
            xdr.endEncoding();
            os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
            xdr.close();
            os.flush();
        } catch (IOException e) {
            throw new PlayerException("[Simulation] : Couldn't send command: "
                    + e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException("[Simulation] : Error while XDR-encoding command: "
                    + e.toString(), e);
        }
    }

    /**
     * Configuration request: set 2D pose of a named simulation object.
     *<br><br>
     * To set the pose of an object in a simulator, send a
     * PLAYER_SIMULATION_REQ_SET_POSE2D request. Response will be null.
     * <br><br>
     * See the player_simulation_pose2d_req structure from player.h
     */
    public void set2DPose(String identifier, PlayerPose2d pp) {
        String temp = identifier + "\0";
        if (identifier.length() > PLAYER_SIMULATION_IDENTIFIER_MAXLEN) {
            temp = identifier.substring(0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        }
        try {
            int leftOvers = 0;
            // Take care of the residual zero bytes
            if ((temp.length() % 4) != 0) {
                leftOvers = 4 - (temp.length() % 4);
            }

            int size = 8 + 24 + temp.length() + leftOvers;

            sendHeader(PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_SET_POSE2D, size);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream(8);
            xdr.beginEncoding(null, 0);
            xdr.xdrEncodeInt(temp.length());
            xdr.xdrEncodeInt(temp.length());
            xdr.endEncoding();
            os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
            xdr.close();

            os.writeBytes(temp);
            byte[] buf = new byte[leftOvers];
            os.write(buf, 0, leftOvers);

            xdr = new XdrBufferEncodingStream(24);
            xdr.beginEncoding(null, 0);
            xdr.xdrEncodeDouble(pp.getPx());
            xdr.xdrEncodeDouble(pp.getPy());
            xdr.xdrEncodeDouble(pp.getPa());
            xdr.endEncoding();
            os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
            xdr.close();
            os.flush();
        } catch (IOException e) {
            throw new PlayerException("[Simulation] : Couldn't request "
                    + "PLAYER_SIMULATION_REQ_SET_POSE2D: " + e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException("[Simulation] : Error while XDR-encoding SET_POSE2D request: "
                    + e.toString(), e);
        }
    }

    /* helper function for get2Dpose and get3Dpose bellow */
    private void getPose(String identifier, String type, int cmd, int extraLen) {
        String temp = identifier+"\0";
        if (identifier.length() > PLAYER_SIMULATION_IDENTIFIER_MAXLEN) {
            temp = identifier.substring(0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        }
        try {
            int leftOvers = 0;
            // Take care of the residual zero bytes
            if ((temp.length() % 4) != 0) {
                leftOvers = 4 - (temp.length() % 4);
            }

            int size = 8 + extraLen + temp.length() + leftOvers;

            sendHeader(PLAYER_MSGTYPE_REQ, cmd, size);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream(8);
            xdr.beginEncoding(null, 0);
            xdr.xdrEncodeInt(temp.length());    // name_count
            xdr.xdrEncodeInt(temp.length());    // array_count
            xdr.endEncoding();
            os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
            xdr.close();

            os.writeBytes(temp);
            byte[] buf = new byte[leftOvers];
            os.write(buf, 0, leftOvers);

            buf = new byte[extraLen];
            os.write(buf, 0, extraLen);
            os.flush();
        } catch (IOException e) {
            throw new PlayerException("[Simulation] : Couldn't request "
                    + "PLAYER_SIMULATION_REQ_GET_POSE" + type + ": " + e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException("[Simulation] : Error while XDR-encoding GET_POSE" + type + " request: "
                    + e.toString(), e);
        }
    }

    /**
     * Configuration request: get 2D pose of a named simulation object.
     *<br><br>
     * To set or get the pose of an object in a simulator, use this message type. If the
     * subtype is PLAYER_SIMULATION_GET_POSE2D, the server will attempt to locate the
     * named object and reply with the same packet with (x,y,a) filled in. For all message
     * subtypes, if the named object does not exist, or some other error occurs, the request
     * should reply NACK.<br><br>
     * See the player_simulation_pose2d_req structure from player.h
     * @param identifier the identifier of the object we want to locate
     */
    public void get2DPose(String identifier) {
        getPose(identifier, "2D", PLAYER_SIMULATION_REQ_GET_POSE2D, 3*8);

//        try {
//            int size = 4 // name_lenght
//                    + 4 //???
//                    + 4 // name
//                    //+ 3 // name trail
//                    + 3 * 8; // coordinates;
//
//            sendHeader(PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D, size);
//
//            byte b = 0;
//            
//            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream(size);
//            xdr.beginEncoding(null, 0);
//            xdr.xdrEncodeInt(2+1);    // name length
//            xdr.xdrEncodeString("o1\0"); // the name 
//            
//            // coordinates
//            for (int i = 0; i < 3*2; i++) {
//                xdr.xdrEncodeByte(b);                
//            }
//            
//            xdr.endEncoding();
//            os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
//            xdr.close();
//            
//            os.flush();
//        } catch (IOException e) {
//            throw new PlayerException("[Simulation] : Couldn't request "
//                    + "PLAYER_SIMULATION_REQ_GET_POSE2D" + ": " + e.toString(), e);
//        } catch (OncRpcException e) {
//            throw new PlayerException("[Simulation] : Error while XDR-encoding GET_POSE2D"+ " request: "
//                    + e.toString(), e);
//        }
    }

    /**
     * Configuration request: get 3D pose of a named simulation object.
     */
    public void get3DPose(String identifier) {
        getPose(identifier, "3D", PLAYER_SIMULATION_REQ_GET_POSE3D, 7 * 8);
    }

    private void handleResponse2D() throws IOException, OncRpcException {
        psp2dreq = new PlayerSimulationPose2dReq();

        // Buffer for reading name_count
        byte[] buffer = new byte[8];
        is.readFully(buffer);

        // Begin decoding the XDR buffer
        XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
        xdr.beginDecoding();
        int nameCount = xdr.xdrDecodeInt();
        xdr.endDecoding();
        xdr.close();

        buffer = new byte[nameCount];
        is.readFully(buffer, 0, nameCount);

        String name = new String(buffer).substring(0, nameCount - 1);
        psp2dreq.setName(name.toCharArray());

        // Take care of the residual zero bytes
        if ((nameCount % 4) != 0) {
            is.readFully(buffer, 0, 4 - (nameCount % 4));
        }

        // Buffer for reading pose
        buffer = new byte[3 * 8];
        is.readFully(buffer);

        PlayerPose2d pp = new PlayerPose2d();
        // Begin decoding the XDR buffer
        xdr = new XdrBufferDecodingStream(buffer);
        xdr.beginDecoding();
        pp.setPx(xdr.xdrDecodeDouble());
        pp.setPy(xdr.xdrDecodeDouble());
        pp.setPa(xdr.xdrDecodeDouble());
        xdr.endDecoding();
        xdr.close();

        psp2dreq.setPose(pp);

        readyPsp2dreq = true;
    }

    private void handleResponse3D() throws IOException, OncRpcException {
        psp3dreq = new PlayerSimulationPose3dReq();

        // Buffer for reading name_count
        byte[] buffer = new byte[8];
        is.readFully(buffer);

        // Begin decoding the XDR buffer
        XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
        xdr.beginDecoding();
        int nameCount = xdr.xdrDecodeInt();
        xdr.endDecoding();
        xdr.close();

        buffer = new byte[nameCount];
        is.readFully(buffer, 0, nameCount);

        String name = new String(buffer).substring(0, nameCount - 1);
        psp3dreq.setName(name.toCharArray());

        // Take care of the residual zero bytes
        if ((nameCount % 4) != 0) {
            is.readFully(buffer, 0, 4 - (nameCount % 4));
        }

        // Buffer for reading pose (7 * 8 = 56)
        buffer = new byte[56];
        is.readFully(buffer, 0, 56);

        PlayerPose3d pp = new PlayerPose3d();
        // Begin decoding the XDR buffer
        xdr = new XdrBufferDecodingStream(buffer);
        xdr.beginDecoding();
        // The position 3D
        pp.setPx(xdr.xdrDecodeDouble());
        pp.setPy(xdr.xdrDecodeDouble());
        pp.setPz(xdr.xdrDecodeDouble());
        pp.setProll(xdr.xdrDecodeDouble());
        pp.setPpitch(xdr.xdrDecodeDouble());
        pp.setPyaw(xdr.xdrDecodeDouble());
        psp3dreq.setPose(pp);
        // The simulation time
        psp3dreq.setSimtime(xdr.xdrDecodeDouble());
        xdr.endDecoding();
        xdr.close();

        readyPsp3dreq = true;
    }

    /**
     * Handle acknowledgement response messages.
     * @param header Player header
     */
    protected void handleResponse(PlayerMsgHdr header) {
        try {
            switch (header.getSubtype()) {
                case PLAYER_SIMULATION_REQ_GET_POSE2D:
                    handleResponse2D();
                    break;
                case PLAYER_SIMULATION_REQ_GET_POSE3D:
                    handleResponse3D();
                    break;
                case PLAYER_SIMULATION_REQ_SET_POSE2D:
                    break;
                default:
                    if (isDebugging) {
                        logger.log(Level.FINEST, "[Simulation][Debug] : "
                                + "Unexpected response " + header.getSubtype()
                                + " of size = " + header.getSize());
                    }
                    break;
            }
        } catch (IOException e) {
            throw new PlayerException("[Simulation] : Error reading payload: "
                    + e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException("[Simulation] : Error while XDR-decoding payload: "
                    + e.toString(), e);
        }
    }
}
